#include "ros/ros.h"
#include "can_msgs/Frame.h"
#include <sstream>
#include "format_data.h"
#include "cyberarm_controller/state_msg.h"
#include "cyberarm_controller/property_msg.h"
#include "cyberarm_controller/arm_state_msg.h"
#include <fstream>

#define P_MIN -12.5f
#define P_MAX 12.5f
#define V_MIN -30.0f
#define V_MAX 30.0f
#define KP_MIN 0.0f
#define KP_MAX 500.0f
#define KD_MIN 0.0f
#define KD_MAX 5.0f
#define T_MIN -12.0f
#define T_MAX 12.0f

#define POS_THRE 0.1

ros::Publisher arm_state_msg_pub;
ros::Publisher property_msg_pub;
ros::Publisher can_msg_pub;
struct exCanIdInfo{
    uint32_t id:8;
    uint32_t data:16;
    uint32_t mode:5;
    uint32_t res:3;
};

#define RAD_DEG 180 / M_PI
#define RAD_S_R_MIN 30 / M_PI

#define rxCanIdEx (((struct exCanIdInfo*)&(msg->id)))
cyberarm_controller::arm_state_msg arm_state_msg;

void chatterCallback(const can_msgs::Frame::ConstPtr &msg)
{    
    uint16_t id = (msg->id & 0xff00) >> 8;
    rxCanIdEx->res = 0;
    if (rxCanIdEx->mode != 0x11 && rxCanIdEx->mode != 0x09 && (id == 1 || id == 2 || id == 3 || id == 4)) // 判断通信类型是否为0x11
    {    
        arm_state_msg.id[id - 1] = id;
        arm_state_msg.pos_estimate[id - 1] = uint_to_float((msg->data[0] << 8) + msg->data[1], P_MIN, P_MAX, 16)*RAD_DEG;
        arm_state_msg.vel_estimate[id - 1] = uint_to_float((msg->data[2] << 8) + msg->data[3], V_MIN, V_MAX, 16)*RAD_S_R_MIN;
        arm_state_msg.torque_estimate[id - 1] = uint_to_float((msg->data[4] << 8) + msg->data[5], T_MIN, T_MAX, 16);
        arm_state_msg.temp[id - 1] = ((msg->data[6] << 8) + msg->data[7]) * 0.10f;
        if(arm_state_msg.id[0] == 1 && arm_state_msg.id[1] == 2 && arm_state_msg.id[2] == 3 && arm_state_msg.id[3] == 4)
        {
            ROS_INFO("pos msg: %f, %f, %f, %f", arm_state_msg.pos_estimate[0], arm_state_msg.pos_estimate[1], 
                arm_state_msg.pos_estimate[2], arm_state_msg.pos_estimate[3]);
            arm_state_msg_pub.publish(arm_state_msg);
            for(unsigned int i = 0; i < 4; i++)
            {
                arm_state_msg.id[i] = 0;
            }
        }
    }
    else
    {
        ROS_INFO("mode incorrect!!!");
    }
    
}

int main(int argc, char **argv)
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "recvmsg_subscriber_node");
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("received_messages", 1000, chatterCallback);
    arm_state_msg_pub = n.advertise<cyberarm_controller::arm_state_msg>("arm_state", 1000);
    ros::Rate r(200);
    while(ros::ok())
    {
        // if(fabs(arm_state_msg.pos_estimate[0]) > POS_THRE || fabs(arm_state_msg.pos_estimate[0]) > POS_THRE || fabs(arm_state_msg.pos_estimate[0]) > POS_THRE || fabs(arm_state_msg.pos_estimate[0]) > POS_THRE)
        // {
        //     n.setParam("zero_mode_allow", true);
        // }
        // else
        // {
        //     n.setParam("zero_mode_allow", false);
        // }
        ros::spinOnce();
        r.sleep();
    }
    
    return 0;
}
